% hw 3-20的关节力矩一起生成了


initial_state = [pi/2 0 0];
is_g_enable = 1;
n_factor = 50;
err = 0.95;
a_sj = simpSingleJoint(initial_state,is_g_enable,n_factor);
a_sj.update_m_l(err);

%用于存放三个力矩
res = zeros(1,length(1:simu_step),3);

for i = 1:simu_step
    %直接用理想的速度和位置去求
    a_sj.joint_state = pos_s_traj_value(i,:);
    a_sj.state = a_sj.joint_state.*a_sj.n_factor;  %角度确实不太对，不过无所谓了，不用的
    %  临时改变主意，直接传惯性力矩，阻尼力矩，干扰力矩绝对值了
    res(1,i,:) = a_sj.inverse_dynamic(0,a_sj.joint_state,a_sj.state);
end

figure;
plot(0:simu_period:(simu_step-1)*simu_period,res(1,:,1)-res(1,:,2)+res(1,:,3));
title("位置s总力矩");
figure;
plot(0:simu_period:(simu_step-1)*simu_period,res(1,:,3));
title("位置s非线性力矩");
figure;
plot(0:simu_period:(simu_step-1)*simu_period,res(1,:,1)-res(1,:,2));
title("位置s线性力矩");

%% 速度S轨迹

for i = 1:simu_step
    %直接用理想的速度和位置去求
    a_sj.joint_state = vel_s_traj_value(i,:);
    a_sj.state = a_sj.joint_state.*a_sj.n_factor;  %角度确实不太对，不过无所谓了，不用的
    %  临时改变主意，直接传惯性力矩，阻尼力矩，干扰力矩绝对值了
    res(1,i,:) = a_sj.inverse_dynamic(0,a_sj.joint_state,a_sj.state);
end

figure;
plot(0:simu_period:(simu_step-1)*simu_period,res(1,:,1)-res(1,:,2)+res(1,:,3));
title("速度S总力矩");
figure;
plot(0:simu_period:(simu_step-1)*simu_period,res(1,:,3));
title("速度S非线性力矩");
figure;
plot(0:simu_period:(simu_step-1)*simu_period,res(1,:,1)-res(1,:,2));
title("速度S线性力矩");